import sensor, image, time
from machine import UART
from pyb import UART
import json
green_threshold   =(62, 77, -19, 9, -24, 5)
#red_threshold   =(54, 100, 11, 74, 2, 51) 胶带
red_threshold =(63, 74, 4, 53, -11, 9)
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_windowing(100,60,160,130)
sensor.set_vflip(True)##上下
sensor.set_hmirror(True)##左右
sensor.set_contrast(-3)
sensor.set_brightness(-1)
sensor.skip_frames(10)
clock = time.clock()
sensor.set_auto_exposure(False,2000)#设置曝光度
sensor.set_auto_whitebal(False)#禁用白平衡
#sensor.set_auto_gain(False)#关闭自动增益

d = {}
i=0
mode=0
rx_buff=[]
state = 0
f=0
uart = UART(3, 115200, timeout_char=1000)

#串口接收函数
def Receive_Prepare(da):
    global state
    if state==0:
        if da == 0x0d:#帧头
            state = 1
        else:
            state = 0
            rx_buff.clear()
    elif state==1:
        rx_buff.append(da)
        mode=da
    else:
        state = 0
        rx_buff.clear()


while(True):
    img = sensor.snapshot()
    green_blobs = img.find_blobs([green_threshold])
    red_blobs = img.find_blobs([red_threshold])

    if(uart.any()>0):
       f=uart.readchar()
       Receive_Prepare(f)
       mode=f


    if mode==0:
        if green_blobs:
            for a in green_blobs:
                img.draw_cross(a[5], a[6])
                print(a[5],a[6],mode)
                data = bytearray([0xb3,0xb3,0,0,0,0,0,0,0,0,a[5],a[6],0,0,0x0d,0x0a])
                uart.write(data)
    if mode==1:
        if green_blobs:
            for a in green_blobs:
                img.draw_cross(a[5], a[6])
            rects=img.find_rects(threshold = 10000)
            if rects:
                for b in rects:
                    img.draw_rectangle(b.rect())
                    i=0
                    for p in b.corners():
                        d[i]=p[0]
                        d[i+1]=p[1]
                        i+=2
                print(d[0],d[1],d[2],d[3],d[4],d[5],d[6],d[7],a[5], a[6],mode)
                data = bytearray([0xb3,0xb3,d[0],d[1],d[2],d[3],d[4],d[5],d[6],d[7],a[5], a[6],0, 0,0x0d,0x0a])
                uart.write(data)
    if mode==2:
        #找绿激光
        for a in green_blobs:
            img.draw_cross(a[5], a[6])
            #找红激光
            for c in red_blobs:
                img.draw_cross(c[5], c[6])
                print(a[5],a[6],c[5],c[6],mode)
            if(green_blobs and red_blobs):
                data = bytearray([0xb3,0xb3,0,0,0,0,0,0,0,0,a[5],a[6],c[5],c[6],0x0d,0x0a])
                uart.write(data)

